/*=================================================================
 * readData.cpp consists of functions that read parameters and
 * point cooridinates from files.
 *
 * Dr.Kanok Weerawong
 * 23/04/2010
 *=================================================================*/
#include "stdafx.h"
#include <iostream>
#include <fstream>
#include <stdlib.h>
#include <math.h>
#include <list>
#include "Markup.h"
using namespace std;

struct TimeStruct {
	double year;
	double month;
	double day;
	double hour;
	double minute;
	double second;
};
struct DimapDataStruct {
	int nRows;
	TimeStruct refTime;
	int refLine;
	double linePeriod;
	double xyLos[8];
	double yawPitchRoll[3];
	double uv[2];
	int nXYZs;
	TimeStruct *tXYZ;
	double *XArray;
	double *YArray;
	double *ZArray;
	int nQuaternions;
	TimeStruct *tQuaternion;
	double *q0Array;
	double *q1Array;
	double *q2Array;
	double *q3Array;
};
struct ImagePointStruct {
	string id;
	bool flag;
	double x;
	double sigmax;
	double y;
	double sigmay;
};
struct GroundPointStruct {
	string id;
	char pointType;
	double X;
	double Y;
	double Z;
	double sigmaX;
	double sigmaY;
	double sigmaZ;
	struct ImagePointStruct *imagePoints;
};

/*=================================================================
 * wstring2String() : convert wstring to string
 * input   : wstring
 * returns : string
 *
 * Dr.Kanok Weerawong
 * 18/05/2010
 *=================================================================*/
string wstring2String(const wstring &inputWstring)
{
	string outputString(inputWstring.begin(), inputWstring.end());
	outputString.assign(inputWstring.begin(), inputWstring.end());
	return outputString;
}

/*=================================================================
 * string2Wstring() : convert string to wstring
 * input   : string
 * returns : wstring
 *
 * Dr.Kanok Weerawong
 * 17/05/2010
 *=================================================================*/
wstring string2Wstring(const string& inputString)
{
	wstring outputWstring(inputString.length(),L' ');
	copy(inputString.begin(), inputString.end(), outputWstring.begin());
	return outputWstring;
}

/*=================================================================
 * wstring2Double() : convert wstring to double
 * input   : wstring
 * returns : the value of the string in double
 *
 * Dr.Kanok Weerawong
 * 23/04/2010
 *=================================================================*/
double wstring2Double(wstring inputWstring)
{
	double dummyDouble;

	string dummyString(inputWstring.begin(), inputWstring.end());
	dummyString.assign(inputWstring.begin(), inputWstring.end());
	dummyDouble = atof(dummyString.c_str());
	return dummyDouble;
}

/*=================================================================
 * wstring2TimeStruct() : convert wstring to timeArray
 * input   : wstring
 * returns : timeArray which is a 6 dimensional array
 *
 * Dr.Kanok Weerawong
 * 23/04/2010
 *=================================================================*/
TimeStruct wstring2TimeStruct(wstring inputWstring)
{
	TimeStruct dummyTime;

	string dummyString(inputWstring.begin(), inputWstring.end());
	dummyString.assign(inputWstring.begin(), inputWstring.end());

	string yearString(dummyString, 0, 4);
	dummyTime.year = atof(yearString.c_str());

	string monthString(dummyString, 5, 2);
	dummyTime.month = atof(monthString.c_str());

	string dayString(dummyString, 8, 2);
	dummyTime.day = atof(dayString.c_str());

	string hourString(dummyString, 11, 2);
	dummyTime.hour = atof(hourString.c_str());

	string minuteString(dummyString, 14, 2);
	dummyTime.minute = atof(minuteString.c_str());

	string secondString(dummyString, 17, 9);
	dummyTime.second = atof(secondString.c_str());

	return dummyTime;
}

/*=================================================================
 * latLongHeight2Xyz() : convert coordinate in the form of
 *                       latitude, longitude, and height in WGS84 to
 *                       geocentric (XYZ) coordinate
 * input   : latitude and longitude values in decimal degrees
 *           and height in meters
 * returns : XYZ (geocentric WGS84)
 *
 * Dr.Kanok Weerawong
 * 13/05/2010
 *=================================================================*/
double *latLongHeight2Xyz(double lat, double lon, double height)
{
	const double PI = atan(1.0)*4;
	const double a  = 6378137.0;
	const double e2 = 0.00669437999014131;
	double deg2rad = PI/180;

	lat = deg2rad*lat;
    lon = deg2rad*lon;
	double N = a/sqrt(1-(e2*sin(lat)*sin(lat)));
	double *XYZ = new double[3];

	XYZ[0] = (N+height)*cos(lat)*cos(lon);
	XYZ[1] = (N+height)*cos(lat)*sin(lon);
	XYZ[2] = (N*(1-e2)+height)*sin(lat);

	return XYZ;
}

/*=================================================================
 * ecef2Enu() : convert from ECEF to ENU coordinate system
 *
 * input   : XYZecef
 * returns : XYZenu
 *
 * Dr.Kanok Weerawong
 * 13/05/2010
 *=================================================================*/
double *ecef2Enu(double latOrigin, double lonOrigin, double *XYZorigin, double *XYZecef)
{
	double vectorX = XYZecef[0]-XYZorigin[0];
	double vectorY = XYZecef[1]-XYZorigin[1];
	double vectorZ = XYZecef[2]-XYZorigin[2];
	double *XYZenu = new double[3];

	XYZenu[0] = -sin(lonOrigin)*vectorX + cos(lonOrigin)*vectorY;
	XYZenu[1] = -sin(latOrigin)*cos(lonOrigin)*vectorX 
				-sin(latOrigin)*sin(lonOrigin)*vectorY
				+cos(latOrigin)*vectorZ;
	XYZenu[2] =  cos(latOrigin)*cos(lonOrigin)*vectorX 
				+cos(latOrigin)*sin(lonOrigin)*vectorY
				+sin(latOrigin)*vectorZ;

	return XYZenu;
}

/*=================================================================
 * readDimap() : Read specific parameters from a DIMAP file
 * returns : DimapDataStruct
 *
 * Dr.Kanok Weerawong
 * 23/04/2010
 *=================================================================*/
DimapDataStruct readDimap(wstring fileName)
{
	CMarkup dimap;
	wstring dummy;
	DimapDataStruct dimapData;
	
	if (!dimap.Load(fileName)){
		cout << "Opening file: ";
		wcout << fileName;
		cout << " failed" << endl;
		exit(1);
	}
	dimap.FindElem(); // DIMAP
	dimap.IntoElem();

// Get the scene center
	dimap.FindElem(MCD_T("Raster_Dimensions"));
	dimap.IntoElem();
	dimap.FindElem(MCD_T("NROWS"));
	dummy = dimap.GetData();
	dimap.OutOfElem();
	dimapData.nRows = (int)wstring2Double(dummy);

// Get into Data_Strip
	dimap.FindElem(MCD_T("Data_Strip"));
	dimap.IntoElem();

// Get into Time_Stamp
	dimap.FindElem(MCD_T("Time_Stamp"));
	dimap.IntoElem();
	dimap.FindElem(MCD_T("REFERENCE_TIME"));
	dummy = dimap.GetData();
	dimapData.refTime = wstring2TimeStruct(dummy);
	dimap.FindElem(MCD_T("REFERENCE_LINE"));
	dummy = dimap.GetData();
	dimapData.refLine = (int)wstring2Double(dummy);
	dimap.FindElem(MCD_T("LINE_PERIOD"));
	dummy = dimap.GetData();
	dimapData.linePeriod = wstring2Double(dummy);
	dimap.OutOfElem();
	dimap.ResetMainPos();

	dimap.FindElem(MCD_T("Ephemeris"));
	dimap.IntoElem();
	dimap.FindElem(MCD_T("Raw_Ephemeris"));
	dimap.IntoElem();
	dimap.FindElem(MCD_T("Point_List"));
	dimap.IntoElem();

// Looping to find the total number of points
	dimapData.nXYZs = 0;
	while ( dimap.FindElem(MCD_T("Point"))){
		dimapData.nXYZs++;
	}
	dimapData.tXYZ = new TimeStruct[dimapData.nXYZs];
	dimapData.XArray = new double[dimapData.nXYZs];
	dimapData.YArray = new double[dimapData.nXYZs];
	dimapData.ZArray = new double[dimapData.nXYZs];

	dimap.ResetMainPos();
	int i = 0;
	while ( dimap.FindElem(MCD_T("Point"))){
		dimap.IntoElem();
		dimap.FindElem(MCD_T("TIME"));
		dummy = dimap.GetData();
		dimapData.tXYZ[i] = wstring2TimeStruct(dummy);

		dimap.ResetMainPos();
		dimap.IntoElem();
		dimap.FindElem(MCD_T("Location"));
		dimap.IntoElem();

		dimap.FindElem(MCD_T("X"));
		dummy = dimap.GetData();
		dimapData.XArray[i] = wstring2Double(dummy);
		dimap.ResetMainPos();

		dimap.FindElem(MCD_T("Y"));
		dummy = dimap.GetData();
		dimapData.YArray[i] = wstring2Double(dummy);
		dimap.ResetMainPos();

		dimap.FindElem(MCD_T("Z"));
		dummy = dimap.GetData();
		dimapData.ZArray[i] = wstring2Double(dummy);
		dimap.ResetMainPos();

		dimap.OutOfElem();// Get up to Location
		dimap.OutOfElem();// Get up to Point
		i++;
	}
	dimap.OutOfElem();// Get up to Point_List
	dimap.OutOfElem();// Get up to Raw_Ephemeris
	dimap.OutOfElem();// Get up to Ephemeris

	dimap.FindElem(MCD_T("Attitudes"));
	dimap.IntoElem();
	dimap.FindElem(MCD_T("Raw_Attitudes"));
	dimap.IntoElem();
	dimap.FindElem(MCD_T("U"));
	dummy = dimap.GetData();
	dimapData.uv[0] = wstring2Double(dummy);
	dimap.ResetMainPos();
	dimap.FindElem(MCD_T("V"));
	dummy = dimap.GetData();
	dimapData.uv[1] = wstring2Double(dummy);
	dimap.ResetMainPos();
	dimap.FindElem(MCD_T("Quaternion_List"));
	dimap.IntoElem();

// Looping to find the total number of points
	dimapData.nQuaternions = 0;
	while ( dimap.FindElem(MCD_T("Quaternion"))){
		dimapData.nQuaternions++;
	}
	dimapData.tQuaternion = new TimeStruct[dimapData.nQuaternions];
	dimapData.q0Array = new double[dimapData.nQuaternions];
	dimapData.q1Array = new double[dimapData.nQuaternions];
	dimapData.q2Array = new double[dimapData.nQuaternions];
	dimapData.q3Array = new double[dimapData.nQuaternions];
	dimap.ResetMainPos();

	i = 0;
	while ( dimap.FindElem(MCD_T("Quaternion"))){
		dimap.IntoElem();
		dimap.FindElem(MCD_T("TIME"));
		dummy = dimap.GetData();
		dimapData.tQuaternion[i] = wstring2TimeStruct(dummy);
		dimap.ResetMainPos();

		dimap.FindElem(MCD_T("Q0"));
		dummy = dimap.GetData();
		dimapData.q0Array[i] = wstring2Double(dummy);
		dimap.ResetMainPos();

		dimap.FindElem(MCD_T("Q1"));
		dummy = dimap.GetData();
		dimapData.q1Array[i] = wstring2Double(dummy);
		dimap.ResetMainPos();

		dimap.FindElem(MCD_T("Q2"));
		dummy = dimap.GetData();
		dimapData.q2Array[i] = wstring2Double(dummy);
		dimap.ResetMainPos();

		dimap.FindElem(MCD_T("Q3"));
		dummy = dimap.GetData();
		dimapData.q3Array[i] = wstring2Double(dummy);
		dimap.ResetMainPos();

		dimap.OutOfElem();// Get up to Quaternion
		i++;
	}
	dimap.OutOfElem();// Get up to Quaternion
	dimap.OutOfElem();// Get up to Raw_Attitudes
	dimap.OutOfElem();// Get up to Attitudes
	dimap.ResetMainPos();

// Get into Polynomial_Look_Angles
	dimap.FindElem(MCD_T("Sensor_Configuration"));
	dimap.IntoElem();
	dimap.FindElem(MCD_T("Instrument_Look_Angles_List"));
	dimap.IntoElem();
	dimap.FindElem(MCD_T("Instrument_Look_Angles"));
	dimap.IntoElem();
	dimap.FindElem(MCD_T("Polynomial_Look_Angles"));
	dimap.IntoElem();

	dimap.FindElem(MCD_T("XLOS_0"));
	dummy = dimap.GetData();
	dimapData.xyLos[0] = wstring2Double(dummy);
	dimap.FindElem(MCD_T("XLOS_1"));
	dummy = dimap.GetData();
	dimapData.xyLos[1] = wstring2Double(dummy);
	dimap.FindElem(MCD_T("XLOS_2"));
	dummy = dimap.GetData();
	dimapData.xyLos[2] = wstring2Double(dummy);
	dimap.FindElem(MCD_T("XLOS_3"));
	dummy = dimap.GetData();
	dimapData.xyLos[3] = wstring2Double(dummy);
	dimap.FindElem(MCD_T("YLOS_0"));
	dummy = dimap.GetData();
	dimapData.xyLos[4] = wstring2Double(dummy);
	dimap.FindElem(MCD_T("YLOS_1"));
	dummy = dimap.GetData();
	dimapData.xyLos[5] = wstring2Double(dummy);
	dimap.FindElem(MCD_T("YLOS_2"));
	dummy = dimap.GetData();
	dimapData.xyLos[6] = wstring2Double(dummy);
	dimap.FindElem(MCD_T("YLOS_3"));
	dummy = dimap.GetData();
	dimapData.xyLos[7] = wstring2Double(dummy);

	dimap.OutOfElem(); // Polynomial_Look_Angles
	dimap.OutOfElem(); // Instrument_Look_Angles
	dimap.OutOfElem(); // Instrument_Look_Angles_List
	dimap.ResetMainPos();

	dimap.FindElem(MCD_T("Instrument_Biases"));
	dimap.IntoElem();

	dimap.FindElem(MCD_T("YAW"));
	dummy = dimap.GetData();
	dimapData.yawPitchRoll[0] = wstring2Double(dummy);
	dimap.FindElem(MCD_T("PITCH"));
	dummy = dimap.GetData();
	dimapData.yawPitchRoll[1] = wstring2Double(dummy);
	dimap.FindElem(MCD_T("ROLL"));
	dummy = dimap.GetData();
	dimapData.yawPitchRoll[2] = wstring2Double(dummy);

	return dimapData;
}


/*=================================================================
 * readCoordinates() : Read image & ground coordinates
 *
 * Format of coordinate file:
 * Image Id		Point Id	Lines	Samples		X		Y		Z
 *	int			string		double	double		double	double	double
 *
 * Dr.Kanok Weerawong
 * 29/04/2010
 *=================================================================*/
GroundPointStruct *readCoordinates(wstring inputFilename, list <wstring> &dimapFilenameList,
								   int &nPoints, int &mImages, int &totalImagePoints)
{
	ifstream inputfp;
	int imageInPoints;
	string dummy, groundPointId, imagePointId;
	char pointType;
	double lines, samples, lat, lon, height;
	list <string> groundPointIdList;
	list<string>::iterator k;

// Open data file for reading
	inputfp.open(inputFilename.c_str());
	if(!inputfp) {
      cerr << "Error: file could not be opened" << endl;
      exit(1);
	}

	// First, read through the file, just to find the number of distinct ground points
	// No other data is stored
	inputfp >> mImages;
	// Read ground point id from first image
	inputfp >> dummy; // DIMAP filename
	inputfp >> imageInPoints; // number of points
	nPoints = imageInPoints;
	for (int j=0 ; j<imageInPoints ; j++){
		inputfp >> imagePointId >> groundPointId >> pointType >> lines >> samples >> lon >> lat >> height;
		groundPointIdList.push_back(groundPointId);
	}
	bool isSameId = false;
	// For the rest of the images, add only new ground point
	for (int i=1 ; i<mImages ; i++){
		inputfp >> dummy; // DIMAP filename
		inputfp >> imageInPoints; // number of points
		for (int j=0 ; j<imageInPoints ; j++){
			inputfp >> imagePointId >> groundPointId >> pointType >> lines >> samples >> lon >> lat >> height;

			// Check if this point is a new ground point
			for (k=groundPointIdList.begin(); k != groundPointIdList.end(); ++k){
				if (!groundPointId.compare(*k)){
					isSameId = true;
					break;
				}
			}	
			if (!isSameId) {
				groundPointIdList.push_back(groundPointId);
			}
		}
	}

	// Next, really read data
	// Create empty groundPoints
	GroundPointStruct *groundPoints = new GroundPointStruct[nPoints];
	int pointIndex = 0;
	for (k=groundPointIdList.begin(); k != groundPointIdList.end(); ++k){
		groundPoints[pointIndex].id = *k;
		// Create imagePoints for each groundPoint with default values
		groundPoints[pointIndex].imagePoints = new ImagePointStruct[mImages];
		for (int l=0 ; l < mImages ; l++){
			groundPoints[pointIndex].imagePoints[l].id = "";
			groundPoints[pointIndex].imagePoints[l].flag = false;
			groundPoints[pointIndex].imagePoints[l].x = 0;
			groundPoints[pointIndex].imagePoints[l].y = 0;
			groundPoints[pointIndex].imagePoints[l].sigmax = 0;
			groundPoints[pointIndex].imagePoints[l].sigmay = 0;
		}
		pointIndex++;
	}
	// Go back to the begining of the file
	inputfp.seekg(ios_base::beg);
	totalImagePoints = 0;
	inputfp >> mImages;
	for (int i=0 ; i<mImages ; i++){
		inputfp >> dummy; // DIMAP filename
		dimapFilenameList.push_back(string2Wstring(dummy));
		inputfp >> imageInPoints; // number of points
		totalImagePoints +=imageInPoints;
		for (int l=0 ; l<imageInPoints ; l++){
			inputfp >> imagePointId >> groundPointId >> pointType >> lines >> samples >> lon >> lat >> height;

			// Find the corresponding point
			for (int j=0 ; j < nPoints ; j++){
				if (!(groundPoints[j].id).compare(groundPointId)){
					groundPoints[j].pointType = pointType;
					// Coordinate conversion: geographic -> geocentric
					double *XYZ = latLongHeight2Xyz(lat, lon, height);
					groundPoints[j].X = XYZ[0];
					groundPoints[j].Y = XYZ[1];
					groundPoints[j].Z = XYZ[2];
					delete [] XYZ;
					if (pointType == 'C') {
						groundPoints[j].sigmaX = 0.5; // meters
						groundPoints[j].sigmaY = 0.5; //
						groundPoints[j].sigmaZ = 0.5; //
					} else {
						groundPoints[j].sigmaX = 300; // meters
						groundPoints[j].sigmaY = 300; //
						groundPoints[j].sigmaZ = 300; //
					}
					groundPoints[j].imagePoints[i].id = imagePointId;
					groundPoints[j].imagePoints[i].flag = true;
					groundPoints[j].imagePoints[i].x = lines;
					groundPoints[j].imagePoints[i].y = samples;
					groundPoints[j].imagePoints[i].sigmax = 5E-06;
					groundPoints[j].imagePoints[i].sigmay = 5E-06;
				}// end if
			}
		}
	}

	inputfp.close();
	return groundPoints;
}